﻿using Microsoft.Kinect;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using System.Net;
using System.Net.Sockets;
using System.Runtime.InteropServices;
using System.Diagnostics;
using System.Drawing;


namespace KinectTest3
{
	/// <summary>
	/// Interaction logic for MainWindow.xaml
	/// </summary>
	public partial class MainWindow : Window
	{
		private byte[] colorPixels = null;
		private ushort[] depthData = null;
		private byte[] depthPixels = null;
		private Point currentMousePosition;
		private CoordinateMapper mapper = null;

		public MainWindow()
		{
            
			InitializeComponent();
		}
		
		private void connect_Click(object sender, RoutedEventArgs e)
		{
			KinectSensor sensor = KinectSensor.GetDefault();
			mapper = sensor.CoordinateMapper;
			Console.WriteLine("capabilities: " + sensor.KinectCapabilities);
			Console.WriteLine("availablity: " + sensor.IsAvailable);
			MultiSourceFrameReader reader = sensor.OpenMultiSourceFrameReader(FrameSourceTypes.Color | FrameSourceTypes.Depth);

			reader.MultiSourceFrameArrived += reader_MultiSourceFrameArrived;
			previewDepthImage.MouseMove += previewDepthImage_MouseMove;
			previewColorImage.MouseMove += previewColorImage_MouseMove;

			sensor.Open();
			MessageBox.Show("connected");
			((Button)sender).IsEnabled = false;
		}

		void previewColorImage_MouseMove(object sender, MouseEventArgs e)
		{
			currentMousePosition = e.GetPosition((Image)sender);
            currentMousePosition.X--;
            currentMousePosition.Y--;
			updateColorTooltip((Image)sender);
		}

		void previewDepthImage_MouseMove(object sender, MouseEventArgs e)
		{
			currentMousePosition = e.GetPosition((Image)sender);
            currentMousePosition.X--;
            currentMousePosition.Y--;
			updateDepthTooltip((Image)sender);
		}

		private void updateDepthTooltip(Image sender)
		{
			int x = (int)currentMousePosition.X;
			int y = (int)currentMousePosition.Y;

			//positionX.Content = x;
			//positionY.Content = y;

			int width = (int)sender.Width;
			int height = (int)sender.Height;

			if(x > width)
				x = width;
			if(y > height)
				y = height;
			if(x < 0)
				x = 0;
			if(y < 0)
				y = 0;

			DepthSpacePoint depthPoint = new DepthSpacePoint();
			depthPoint.X = x;
			depthPoint.Y = y;
			CameraSpacePoint cameraPoint = mapper.MapDepthPointToCameraSpace(depthPoint, depthData[y * width + x]);
			positionX.Content = cameraPoint.X;
			positionY.Content = cameraPoint.Y;
			positionZ.Content = cameraPoint.Z;
		}

		private void updateColorTooltip(Image sender)
		{
			int x = (int)currentMousePosition.X * 2;
			int y = (int)currentMousePosition.Y * 2;

			int width = (int)sender.Width * 2;
			int height = (int)sender.Height * 2;

			if (x > width)
				x = width;
			if (y > height)
				y = height;
			if (x < 0)
				x = 0;
			if (y < 0)
				y = 0;

			ColorSpacePoint colorPoint = new ColorSpacePoint();
			colorPoint.X = x;
			colorPoint.Y = y;
			CameraSpacePoint[] cameraPoints = new CameraSpacePoint[width * height];
			mapper.MapColorFrameToCameraSpace(depthData, cameraPoints);
            CameraSpacePoint cameraPoint;
                cameraPoint = cameraPoints[y * width + x];
			positionX.Content = cameraPoint.X;
			positionY.Content = cameraPoint.Y;
			positionZ.Content = cameraPoint.Z;
		}

		void reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
		{
            //// Acquire and process from from Epiphan Grabber

            Process.Start("C:\\Users\\Maryam\\Downloads\\epiphan_sdk-3.28.0.0009\\epiphan_sdk-3.28.0.0009\\epiphan\\bin\\v2u.exe myimage.png");


            // Create source.
            BitmapImage bi = new BitmapImage();
            // BitmapImage.UriSource must be in a BeginInit/EndInit block.
            bi.BeginInit();
            bi.UriSource = new Uri(@"myframe.png", UriKind.RelativeOrAbsolute);
            bi.EndInit();
            // Set the image source.
            previewColorImage.Source = bi; // change to reference to another displayed image
            

            ////Acquire and process frame from Kinect

			// Get a reference to the multi-frame
			var reference = e.FrameReference.AcquireFrame();

			// Open color frame
            //using (var frame = reference.ColorFrameReference.AcquireFrame())
            //{
            //    if (frame != null)
            //    {
            //        ImageSource source = ToBitmap(frame);
            //        //previewColorImage.Width = source.Width;
            //        //previewColorImage.Height = source.Height;
            //        previewColorImage.Source = source;
            //    }
            //}

			// Open depth frame
			using (var frame = reference.DepthFrameReference.AcquireFrame())
			{
				if (frame != null)
				{
					ImageSource source = ToBitmap(frame);
					previewDepthImage.Width = source.Width;
					previewDepthImage.Height = source.Height;
					previewDepthImage.Source = source;

					updateDepthTooltip(previewDepthImage);
				}
			}
		}

		private ImageSource ToBitmap(ColorFrame frame)
		{
			int width = frame.FrameDescription.Width;
			int height = frame.FrameDescription.Height;

			if (colorPixels == null)
				colorPixels = new byte[width * height * ((PixelFormats.Bgr32.BitsPerPixel + 7) / 8)];

			if (frame.RawColorImageFormat == ColorImageFormat.Bgra)
			{
				frame.CopyRawFrameDataToArray(colorPixels);
			}
			else
			{
				frame.CopyConvertedFrameDataToArray(colorPixels, ColorImageFormat.Bgra);
			}

			int stride = width * PixelFormats.Bgr32.BitsPerPixel / 8;

			return BitmapSource.Create(width, height, 96, 96, PixelFormats.Bgr32, null, colorPixels, stride);
		}

		private ImageSource ToBitmap(DepthFrame frame)
		{
			int width = frame.FrameDescription.Width;
			int height = frame.FrameDescription.Height;

			ushort minDepth = frame.DepthMinReliableDistance;
			ushort maxDepth = frame.DepthMaxReliableDistance;

			depthData = new ushort[width * height];
			depthPixels = new byte[width * height * (PixelFormats.Bgr32.BitsPerPixel + 7) / 8];

			frame.CopyFrameDataToArray(depthData);

			int colorIndex = 0;
			for (int depthIndex = 0; depthIndex < depthData.Length; ++depthIndex)
			{
				ushort depth = depthData[depthIndex];
				byte intensity = (byte)(depth >= minDepth && depth <= maxDepth ? depth : 0);

				depthPixels[colorIndex++] = intensity; // Blue
				depthPixels[colorIndex++] = intensity; // Green
				depthPixels[colorIndex++] = intensity; // Red

				++colorIndex;
			}

			int stride = width * PixelFormats.Bgr32.BitsPerPixel / 8;

			return BitmapSource.Create(width, height, 96, 96, PixelFormats.Bgr32, null, depthPixels, stride);
		} 
	}
}

